#include "clamp.h"
#include "crate.h"
#include <iostream>

const float Clamp::maxRotationAngle =  PI/2;
const float Clamp::rotationSpeed =  0.05f;


Clamp::Clamp(const Cube& cube,const Point& dir,const std::map<std::string,ObjectModelPtr>& models)
	: 	SceneObject(cube,dir,models.find(Clamp::name())->second),
		Grabber(Cube(cube).translate(0,0,-1)){
	this->state = Clamp::OPEN;

	ClampSide* clampSide1 = new ClampSideA(dir,models.find(ClampSideA::name())->second);
	ClampSide* clampSide2 = new ClampSideB(dir,models.find(ClampSideB::name())->second);
	clampSide1->setParent(this);
	clampSide2->setParent(this);

	this->addObject(ClampSideA::name(),clampSide1);
	this->addObject(ClampSideB::name(),clampSide2);

	this->zrotatedAngle = 0;
}

Clamp::~Clamp(){
}

void Clamp::move(float dx,float dy,float dz){
	SceneObject::move(dx,dy,dz);
	this->movePosition(dx,dy,dz);

	(this->findObject(ClampSideA::name())).move(dx,dy,dz);
	(this->findObject(ClampSideB::name())).move(dx,dy,dz);

	if(this->getGrabbed()){
	this->findObject(Grabable::name()).move(dx,dy,dz);
	}
}

void Clamp::rotate(float angle){
	Matrix4x4 rot(angle,Point(0,0,1));
	this->direction = rot*this->direction;

	this->zrotatedAngle += angle;
	this->dimensions.rotate(this->zrotatedAngle,this->dimensions.center(),Point(0,0,1));
	this->rotatePositionZ(angle);

	if(this->getGrabbed()){
		dynamic_cast<Crate&>(this->findObject(Grabable::name())).rotate(angle);
	}
}

void Clamp::setModelMatrix(){
	Point tr = this->dimensions.center();
	glTranslatef(tr.x1,tr.x2,tr.x3);
	this->getParent().setSubModelMatrix(Clamp::name());
	glRotatef(this->zrotatedAngle*180/PI,0.0f,0.0f,1.0f);
	glScalef(this->dimensions.width(),this->dimensions.depth(),this->dimensions.height());

}

void Clamp::setSubModelMatrix(const std::string& name){
	if(name==Crate::name()){
		this->getParent().setSubModelMatrix(name);
	}else{
		Point tr = this->dimensions.center();
		glTranslatef(tr.x1,tr.x2,tr.x3);
		this->getParent().setSubModelMatrix(Clamp::name());
		glRotatef(this->zrotatedAngle*180/PI,0.0f,0.0f,1.0f);
		glScalef(this->dimensions.width(),this->dimensions.depth(),this->dimensions.height());
	}
}

void Clamp::addObject(const std::string& name,SceneObject* obj){
	SceneObject::addObject(name,obj);

	if(name!=ClampSideA::name() && name!=ClampSideB::name()) {
		Point diff = this->dimensions.center()-obj->getDimensions().center();
		dynamic_cast<Crate&>(*obj).move(diff.x1,diff.x2,diff.x3-this->dimensions.width()/1.5);
		obj->setDirection(this->direction);
	}


}

void Clamp::open(){
	if(this->state==Clamp::CLOSING || this->state==Clamp::CLOSED)
		this->state=Clamp::OPENING;
}

void Clamp::close(){
	if(this->state==Clamp::OPENING || this->state==Clamp::OPEN)
		this->state=Clamp::CLOSING;
}

bool Clamp::isOpen(){
	return this->state==Clamp::OPEN;
}

bool Clamp::isClosed(){
	return this->state==Clamp::CLOSED;
}

void Clamp::timeStep(){
	ClampSide& side1 = dynamic_cast<ClampSide&>(this->findObject(ClampSideA::name()));
	ClampSide& side2 = dynamic_cast<ClampSide&>(this->findObject(ClampSideB::name()));

	if(this->state==Clamp::CLOSING){
		if(side1.rotationAngle>=Clamp::maxRotationAngle){
			this->state = Clamp::CLOSED;
			side1.rotationAngle=(Clamp::maxRotationAngle);
			side2.rotationAngle=(Clamp::maxRotationAngle);
			this->grab();
			if(this->getGrabbed()){
				Crate* grabbed = static_cast<Crate*>(this->getGrabbed());
				grabbed->setParent(this);
				this->addObject(Grabable::name(),grabbed);
			}else{
				this->state = Clamp::OPENING;
			}
		}else{
			side1.rotationAngle+=(Clamp::rotationSpeed);
			side2.rotationAngle+=(Clamp::rotationSpeed);
		}
	}else if(this->state==Clamp::OPENING){
		if(side1.rotationAngle<=0){
			this->state = Clamp::OPEN;
			side1.rotationAngle=0;
			side2.rotationAngle=0;
		}else{
			side1.rotationAngle+=(-1.0f*Clamp::rotationSpeed);
			side2.rotationAngle+=(-1.0f*Clamp::rotationSpeed);
		}
	}
}
